#include "RigExpertZeroII_UART.h"

RigExpertZeroII_UART::RigExpertZeroII_UART(int RX_Pin, int TX_Pin)
{
	m_ZERO = new SoftwareSerial(RX_Pin, TX_Pin);
	m_Z0 = 50.0;
	m_R = 0;
	m_X = 0;
	m_SWR = 0;
	m_RL = 0;
	m_Phase = 0;
	m_Rho = 0;
	m_isInited = false;
	m_majorVersion = 0;
	m_minorVersion = 0;
	m_hwRevision = 0;
	m_serialNumber = 0;
}

bool RigExpertZeroII_UART::startZeroII()
{
    m_ZERO->begin(38400); // init ZEROII side UART
	m_ZERO->flush();
	delay(500);
	if(getFwVersion())
	{
		m_isInited = true;
		return true;
	}
	return false;
}

bool RigExpertZeroII_UART::getFwVersion()
{
	char command[3] = {0xE5,0xB5,0x4A};
	m_ZERO->write(command,3);
	bool wait = true;
	long failCounter = 0;
	unsigned char buff[9];
	while(wait)
	{
		if(failCounter++ >= 500000)
			wait = false;
		if(m_ZERO->available() > 8)
		{
			wait = false;
			m_ZERO->readBytes(buff, 9);
			int crc = crc8_buffer(buff,7);
			if((int)buff[7] != crc)
			{
				return false;
			}
			int invcrc = crc ^= 0xFF;
			if((int)buff[8] != invcrc)
			{
				return false;
			}
			
			m_majorVersion = buff[0];
			m_minorVersion = buff[1];
			m_hwRevision = buff[2];
			((unsigned char*)&m_serialNumber)[0] = buff[3];
			((unsigned char*)&m_serialNumber)[1] = buff[4];
			((unsigned char*)&m_serialNumber)[2] = buff[5];
			((unsigned char*)&m_serialNumber)[3] = buff[6];
			return true;
		}
	}
	return false;(m_majorVersion != 0 && m_minorVersion != 0);
}

unsigned char RigExpertZeroII_UART::getMajorVersion()
{
	return m_majorVersion;
}

unsigned char RigExpertZeroII_UART::getMinorVersion()
{
	return m_minorVersion;
}

unsigned char RigExpertZeroII_UART::getHwRevision()
{
	return m_hwRevision;
}

unsigned long RigExpertZeroII_UART::getSerialNumber()
{
	return m_serialNumber;
}

bool RigExpertZeroII_UART::startMeasure(int32_t fq)
{
	if(!m_isInited)
	{
		return false;
	}
	if(m_ZERO == NULL)
	{
		return false;
	}
	if(fq > MAX_FQ)
	{
		fq = MAX_FQ;
	}
	
	bool result = false;
	byte command[7] = {0x6D,0x00,0x00,0x00,0x00,0x00,0x00};

	command[1] = ((unsigned char*)&fq)[0];
	command[2] = ((unsigned char*)&fq)[1];
	command[3] = ((unsigned char*)&fq)[2];
	command[4] = ((unsigned char*)&fq)[3];
	int crc = crc8_buffer(command,5);
	command[5] = crc;
	command[6] = crc ^= 0xFF;

	m_ZERO->write(command,7);

	bool wait = true;
	int failCounter = 0;
	while(wait)
	{
		delay(30);
		if(failCounter++ >= 5)
		{
		  wait = false;
		  return result;
		}
		if(getStatus() == ZEROII_STATUS_READY)
		{
		  result = readRX();
		  wait = false;
		}
	}
	return result;
}

bool RigExpertZeroII_UART::readRX()
{
	bool wait = true;
	long failCounter = 0;
	while(wait)
	{
		if(failCounter++ >= 500000)
			wait = false;
		if(m_ZERO->available() > 9)
		{
			wait = false;
			unsigned char buff[10];
			m_ZERO->readBytes(buff, 10);
			unsigned char crc = crc8_buffer(buff,8);
			if(buff[8] != crc)
			{
				return false;
			}
			int invcrc = crc ^= 0xFF;
			if((int)buff[9] != invcrc)
			{
				return false;
			}
			((unsigned char*)&m_R)[0] = buff[0];
			((unsigned char*)&m_R)[1] = buff[1];
			((unsigned char*)&m_R)[2] = buff[2];
			((unsigned char*)&m_R)[3] = buff[3];

			((unsigned char*)&m_X)[0] = buff[4];
			((unsigned char*)&m_X)[1] = buff[5];
			((unsigned char*)&m_X)[2] = buff[6];
			((unsigned char*)&m_X)[3] = buff[7];
		}
	}
	return true;
}

unsigned char RigExpertZeroII_UART::getStatus()
{
	char command[3] = {0x5A,0x81,0x7E};
	m_ZERO->write(command,3);
	bool wait = true;
	long failCounter = 0;
	unsigned char buff[3];
	while(wait)
	{
		if(failCounter++ >= 500000)
			wait = false;
		if(m_ZERO->available() > 2)
		{
			wait = false;
			m_ZERO->readBytes(buff, 3);
			int crc = crc8_buffer(buff,1);
			if((int)buff[1] != crc)
			{
				return ZEROII_STATUS_ERROR;
			}
			int invcrc = crc ^= 0xFF;
			if((int)buff[2] != invcrc)
			{
				return ZEROII_STATUS_ERROR;
			}
		}
	}
	return buff[0];
}

double RigExpertZeroII_UART::getZ0()
{
	return m_Z0;
}

void RigExpertZeroII_UART::setZ0(double Z0)
{
	m_Z0 = Z0;
}

unsigned char RigExpertZeroII_UART::crc8_buffer(const unsigned char *buf, long len)
{
	unsigned char crc8_calculation_register = 0;

	while(len-- > 0)
	{
		crc8_calculation_register = crc8_table[crc8_calculation_register ^ (*buf)];
		crc8_calculation_register &= 0xFF;
		buf++;
	}
	/* Return the CRC computed value */
	return crc8_calculation_register;
}

bool RigExpertZeroII_UART::computeSWR(double Z0, double R, double X)
{
    if (R <= 0)
    {
        R = 0.001;
    }
    double SWR, Gamma;
    double XX = X * X;
    double denominator = (R + Z0) * (R + Z0) + XX;

    if (denominator == 0)
    {
        return false;
    }
    Gamma = sqrt(((R - Z0) * (R - Z0) + XX) / denominator);
    if (Gamma == 1.0)
    {
        return false;
    }
    SWR = (1 + Gamma) / (1 - Gamma);

    if ((SWR > 200) || (Gamma > 0.99))
    {
        SWR = 200;
    } else if (SWR < 1)
    {
        SWR = 1;
    }
	m_SWR = SWR;
    

    m_RL = -20 * log10(Gamma);

    return true;
}

double RigExpertZeroII_UART::computeZ (double R, double X)
{
    return sqrt((R*R) + (X*X));
}

void RigExpertZeroII_UART::computePhase (double R, double X)
{
    double Z0 = m_Z0;
    double Rnorm = R/Z0;
    double Xnorm = X/Z0;
    double Denom = (Rnorm+1)*(Rnorm+1)+Xnorm*Xnorm;
    double RhoReal = ((Rnorm-1)*(Rnorm+1)+Xnorm*Xnorm)/Denom;
    double RhoImag = 2*Xnorm/Denom;
    m_Phase = atan2(RhoImag, RhoReal) / M_PI * 180.0;
    m_Rho = sqrt(RhoReal*RhoReal+RhoImag*RhoImag);
}

double RigExpertZeroII_UART::getR()
{
    return m_R;
}

double RigExpertZeroII_UART::getRp()
{
	return m_R*(1+m_X*m_X/m_R/m_R);
}

double RigExpertZeroII_UART::getX()
{
    return m_X;
}

double RigExpertZeroII_UART::getXp()
{
	return m_X*(1+m_R*m_R/m_X/m_X);
}

double RigExpertZeroII_UART::getSWR()
{
	computeSWR(m_Z0, m_R, m_X);
    return m_SWR;
}

double RigExpertZeroII_UART::getRL()
{
	computeSWR(m_Z0, m_R, m_X);
    return m_RL;
}

double RigExpertZeroII_UART::getZ()
{
	return computeZ(m_R, m_X);
}

double RigExpertZeroII_UART::getPhase()
{
	computePhase(m_R, m_X);
	return m_Phase;
}

double RigExpertZeroII_UART::getRho()
{
	computePhase(m_R, m_X);
	return m_Rho;
}
